clear; clc
%%
s=tf('s');
gainY = 0.035; gainZ = 0.110;
order = 8; tau=ziBW2TC(2.348, 8) ;tList = 0:0.003:1;

GY=gainY/(1+tau*s)^order;
GZ=gainZ/(1+tau*s)^order;
CY=optimPID0(GY,3,4);
CZ=optimPID0(GZ,3,4);
%%
figure; 
subplot(3, 1, 1);
step(GY*exp(-20e-3*s), tList); grid on;
title('Open-loop response Y');

subplot(3, 1, 2);
step(feedback(CY*GY, 1), tList); grid on;
title(sprintf('ref. track, Y d=%5.3f, p=%5.3f, i=%5.3f', CY.Numerator{1}));

subplot(3, 1, 3);
step(feedback(GY, CY), tList);grid on;
title('noise rejection');

%%
figure; 
subplot(3, 1, 1);
step(GZ*exp(-20e-3*s), tList); grid on;
title('Open-loop response Z');

subplot(3, 1, 2);
step(feedback(CZ*GZ, 1), tList); grid on;
title(sprintf('ref. track, Z d=%5.3f, p=%5.3f, i=%5.3f', CZ.Numerator{1}));

subplot(3, 1, 3);
step(feedback(GZ, CZ), tList);grid on;
title('noise rejection');